cmake_minimum_required(VERSION 2.8.3)
project(sensor_fusion)

######################
### Cmake flags
######################
add_definitions(-w)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++14")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")

include(FindOpenMP)
if(OPENMP_FOUND)
  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
  set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
else(OPENMP_FOUND)
  message("ERROR: OpenMP could not be found.")
endif(OPENMP_FOUND)

######################
### Packages
######################
find_package(catkin REQUIRED COMPONENTS
    tf
    roscpp
    rospy
    roslib
    # msg
    std_msgs
    sensor_msgs
    geometry_msgs
    nav_msgs
    # cv
    cv_bridge
    # pcl
    pcl_conversions
    # msg generation
    message_generation
)

find_package(PCL REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Ceres REQUIRED)
find_package(GTSAM REQUIRED QUIET)
find_package(Boost REQUIRED COMPONENTS filesystem program_options system serialization regex timer date_time thread)

######################
### Message generation
######################
add_message_files(
  DIRECTORY msg
  FILES
  cloud_info.msg
)

generate_messages(
  DEPENDENCIES
  geometry_msgs
  std_msgs
  nav_msgs
  sensor_msgs
)

######################
### Catkin
######################
catkin_package(
    DEPENDS PCL GTSAM
)

include_directories(
    ${catkin_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
    ${OpenCV_INCLUDE_DIRS}
    ${CERES_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
    ${GTSAM_INCLUDE_DIR}
)

link_directories(
    ${PCL_LIBRARY_DIRS}
    ${OpenCV_LIBRARY_DIRS}
    ${GTSAM_LIBRARY_DIRS}
)

######################
### visual odometry
######################
# file(GLOB visual_feature_files
#     "src/visual_odometry/visual_feature/*.cpp"
#     "src/visual_odometry/visual_feature/camera_models/*.cc"
# )
# file(GLOB visual_odometry_files
#     "src/visual_odometry/visual_estimator/*.cpp"
#     "src/visual_odometry/visual_estimator/factor/*.cpp"
#     "src/visual_odometry/visual_estimator/initial/*.cpp"
#     "src/visual_odometry/visual_estimator/utility/*.cpp"
# )
# file(GLOB visual_loop_files
#     "src/visual_odometry/visual_loop/*.cpp"
#     "src/visual_odometry/visual_loop/utility/*.cpp"
#     "src/visual_odometry/visual_loop/ThirdParty/*.cpp"
#     "src/visual_odometry/visual_loop/ThirdParty/DBoW/*.cpp"
#     "src/visual_odometry/visual_loop/ThirdParty/DUtils/*.cpp"
#     "src/visual_odometry/visual_loop/ThirdParty/DVision/*.cpp"
#     "src/visual_odometry/visual_feature/camera_models/*.cc"
# )
# # Visual Feature Tracker
# add_executable(${PROJECT_NAME}_visual_feature ${visual_feature_files})
# target_link_libraries(${PROJECT_NAME}_visual_feature ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
# # Visual Odometry
# add_executable(${PROJECT_NAME}_visual_odometry ${visual_odometry_files})
# target_link_libraries(${PROJECT_NAME}_visual_odometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 
# # Visual Lopp
# add_executable(${PROJECT_NAME}_visual_loop ${visual_loop_files})
# target_link_libraries(${PROJECT_NAME}_visual_loop ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 

######################
### lidar odometry
######################

# # IMU Preintegration
# add_executable(${PROJECT_NAME}_imuPreintegration src/lidar_odometry/imuPreintegration.cpp)
# target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam)
# # Range Image Projection
# add_executable(${PROJECT_NAME}_imageProjection src/lidar_odometry/imageProjection.cpp)
# add_dependencies(${PROJECT_NAME}_imageProjection ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
# target_link_libraries(${PROJECT_NAME}_imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
# # Feature Association
# add_executable(${PROJECT_NAME}_featureExtraction src/lidar_odometry/featureExtraction.cpp)
# add_dependencies(${PROJECT_NAME}_featureExtraction ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
# target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
# # Mapping Optimization
# add_executable(${PROJECT_NAME}_mapOptmization src/lidar_odometry/mapOptmization.cpp)
# add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
# target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS})
# target_link_libraries(${PROJECT_NAME}_mapOptmization PRIVATE ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam)

######################
### Fusion
######################

add_executable(${PROJECT_NAME}_Fusion_LidarPreProcess src/fusion/lidarPreProcess.cpp src/fusion/utility.h)
target_compile_options(${PROJECT_NAME}_Fusion_LidarPreProcess PRIVATE ${OpenMP_FLAGS})
target_link_libraries(${PROJECT_NAME}_Fusion_LidarPreProcess ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_LIBS})


file(GLOB lidar_odometry_files
    "src/fusion/nano_gicp/*.cc"
    "src/fusion/nano_gicp/*.hpp"
    "src/fusion/nano_gicp/*/*.cc"
    "src/fusion/nano_gicp/*/*.hpp"
    "src/fusion/utility.h"
    "src/fusion/lidarFactorGtsam.hpp"
)
add_executable(${PROJECT_NAME}_Fusion_LidarOdometry src/fusion/lidarOdometry.cpp ${lidar_odometry_files})
target_compile_options(${PROJECT_NAME}_Fusion_LidarOdometry PRIVATE ${OpenMP_FLAGS})
target_link_libraries(${PROJECT_NAME}_Fusion_LidarOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_LIBS} gtsam)